(in-package :gcode)

(defun bezier-to-vertices (bezier)
  (with-slots (a b u v) bezier
    (list (sdl:point :x (2d-point-x a)
		     :y (2d-point-y a))
	  (sdl:point :x (2d-point-x u)
		     :y (2d-point-y u))
	  (sdl:point :x (2d-point-x v)
		     :y (2d-point-y v))
	  (sdl:point :x (2d-point-x b)
		     :y (2d-point-y b)))))

(defun transform-point (p)
  (let ((x-y (transform-vector (2d-point-x p) (2d-point-y p) *current-transform*)))
    (2dp (first x-y) (second x-y))))

(defun draw-arc (arc &key (color sdl:*default-color*))
  (with-slots (a b centre direction) arc
    (let ((a (transform-point a))
	  (b (transform-point b))
	  (centre (transform-point centre)))
      (let* ((s1 (make-line :a centre :b a))
	   (s2 (make-line :a centre :b b))
	   (angle (arc-angle arc))
	   (unit (make-line :a (2dp 0 0) :b (2dp 1 0)))
	   (a1 (angle-2-segments-directed unit s1))
	   (diff (case direction
		   (:cw (- angle))
		   (:ccw angle)))
	   (r (line-length s1))
	   (cx (2d-point-x centre))
	   (cy (2d-point-y centre))
	   (steps (* (abs diff) r))
	   (inc (/ diff steps)))
      #+nil(format t "angle: ~A a1: ~A a2: ~A r: ~A diff: ~A inc: ~A~%"
	      (radians-to-deg angle)
		   (radians-to-deg a1)
		   (radians-to-deg a1)
		   r
		   (radians-to-deg diff)
		   (radians-to-deg inc))
      (do ((angle a1 (+ angle inc))
	   (cnt 0 (1+ cnt)))
	  ((>= cnt steps))
	;;	(format t "angle; ~A~%" (radians-to-deg angle))
	(sdl:draw-pixel (sdl:point :x (+ cx (* (cos angle) r)) :y (+ cy (* (sin angle) r)))
			:color color))))))

(defun my-draw-line (line &key (color sdl:*default-color*))
  (let ((ax (2d-point-x (line-a line)))
	(ay (2d-point-y (line-a line)))
	(bx (2d-point-x (line-b line)))
	(by (2d-point-y (line-b line))))
    (let ((ax-y (transform-vector ax ay *current-transform*))
	  (bx-y (transform-vector bx by *current-transform*)))
      (sdl:draw-line (sdl:point :x (first ax-y)
				:y (second ax-y))
		     (sdl:point :x (first bx-y)
				:y (second bx-y))
		     :color color))))

(defun my-draw-pixel (x y &key color)
  (let ((x-y (transform-vector x y *current-transform*)))
    (sdl:draw-pixel (sdl:point :x (first x-y) :y (second x-y)) :color color)))

(defun my-bezier-to-vertices (bezier)
  (with-slots (a b u v) bezier
    (mapcar #'transform-sdl-point
	    (list (sdl:point :x (2d-point-x a)
			     :y (2d-point-y a))
		  (sdl:point :x (2d-point-x u)
			     :y (2d-point-y u))
		  (sdl:point :x (2d-point-x v)
			     :y (2d-point-y v))
		  (sdl:point :x (2d-point-x b)
			     :y (2d-point-y b))))))

(defun transform-sdl-point (point)
  (let ((x-y (transform-vector (sdl:x point) (sdl:y point) *current-transform*)))
    (sdl:point :x (first x-y) :y (second x-y))))

(defun my-draw-bezier (bezier &key color)
  (sdl:draw-bezier (my-bezier-to-vertices bezier) :color color :style :solid))

(defun test-draw-arc ()
  (with-sdl-draw ()
    (let ((a1 (make-arc :a (2dp 200 100) :b (2dp 100 200)
			:direction :cw :centre (2dp 100 100)))
	  (a2 (make-arc :a (2dp 200 100) :b (2dp 100 200)
			:direction :ccw :centre (2dp 100 100)))
	  (a3 (make-arc :a (2dp 205 30)
			:b (2dp 200 25)
			:centre (2dp 200 30)
			:direction :cw)))
      #+nil(sdl:with-color (sdl:*default-color* (sdl:color :r 255 :g 0 :b 0))
	(draw-arc a1))
      #+nil(sdl:with-color (sdl:*default-color* (sdl:color :r 0 :g 255 :b 0))
	(draw-arc a2))
      (sdl:with-color (sdl:*default-color* (sdl:color :r 0 :g 0 :b 255))
	(draw-arc a3)))))

(defun draw-curve (curve &key (color sdl:*default-color*))
  (dolist (i curve)
    (cond ((typep i 'bezier)
	   (my-draw-bezier i :color color))
	  ((typep i 'line)
	   (with-slots (a b) i
	     (my-draw-line i :color color)))
	  ((typep i 'arc)
	   (draw-arc i :color color)))))

(defmacro with-sdl-draw (() &rest body)
  `(sdl:with-init ()
     (sdl:window 640 480)
     (setf (sdl:frame-rate) -1)
     (sdl:clear-display (sdl:color :r 0 :g 0 :b 0))
     
     (sdl:with-color (sdl:*default-color* (sdl:color :r 255 :g 255 :b 255))
       ,@body)
     (sdl:update-display)
     (sdl:with-events ()
      (:quit-event () t))))

(defun sdl-curve (curve)
  (sdl:with-init ()
    (sdl:window 1240 480)
    (setf (sdl:frame-rate) -1)
    (sdl:clear-display (sdl:color :r 0 :g 0 :b 0))

    (sdl:with-color (col (sdl:color :r 255 :g 255 :b 255))
      (draw-curve (curve-to-arcs curve) :color col))
    (sdl:update-display)
    (sdl:with-events ()
      (:quit-event () t))))

(defun sdl-curves (curves)
  (sdl:with-init ()
    (sdl:window 1240 480)
    (setf (sdl:frame-rate) -1)
    (sdl:clear-display (sdl:color :r 0 :g 0 :b 0))

    (sdl:with-color (col (sdl:color :r 255 :g 255 :b 255))
      (dolist (curve curves)
	(draw-curve (curve-to-arcs curve) :color col)))
    (sdl:update-display)
    (sdl:with-events ()
      (:quit-event () t))))

